#include <math.h>
#include "ColorConvert.h"

/* #include "WProgram.h" */
float MAX(float r, float g, float b) {
	float max = r;
	if (g > max) {
		max = g;
	}
	if (b > max) {
		max = b;
	}
	return max;
}

float MIN(float r, float g, float b) {
	float min = r;
	if (g < min) {
		min = g;
	}
	if (b < min) {
		min = b;
	}
	return min;
}

/**
 *  [ Y ]     [ 0.299   0.587   0.114 ] [ R ]
 *  [ I ]  =  [ 0.596  -0.275  -0.321 ] [ G ]
 *  [ Q ]     [ 0.212  -0.523   0.311 ] [ B ]	
 */
YIQ RGBtoYIQ(RGB color) {
	YIQ yiq;
	yiq.y = (.299 * color.r) + (.587 * color.g) + (.114 * color.b);
	yiq.i = (.596 * color.r) + (-.275 * color.g) + (-.321 * color.b);
	yiq.q = (.212 * color.r) + (-.523 * color.g) + (.311 * color.b);
	return yiq;
}

/**
 *  [ R ]     [ 1   0.956   0.621 ] [ Y ]
 *  [ G ]  =  [ 1  -0.272  -0.647 ] [ I ]
 *  [ B ]     [ 1  -1.105   1.702 ] [ Q ]
 */
RGB YIQtoRGB(YIQ color) {
	RGB rgb;
	rgb.r = (1 * color.y) + (.956 * color.i) + (.621 * color.q);
	rgb.g = (1 * color.y) + (-.272 * color.i) + (-.647 * color.q);
	rgb.b = (1 * color.y) + (-1.105 * color.i) + (1.702 * color.q);
	return rgb;
}

RGB HSVtoRGB(HSV color, int maxValue) {
	RGB converted;
	float r;
	float g;
	float b;

	if (color.s == 0) {
		// unsaturated!
		r = color.v;
		g = color.v;
		b = color.v;
	} else {
		float h = (360 * color.h) / 60;
		int i = floor(h);
		float f = h - i;
		float p = color.v * (1 - color.s);
		float q = color.v * (1 - color.s * f);
		float t = color.v * (1 - color.s * (1 - f));

		switch (i) {
			case 0:
				r = color.v;
				g = t;
				b = p;
				break;
			case 1:
				r = q;
				g = color.v;
				b = p;
				break;
			case 2:
				r = p;
				g = color.v;
				b = t;
				break;
			case 3:
				r = p;
				g = q;
				b = color.v;
				break;
			case 4:
				r = t;
				g = p;
				b = color.v;
				break;
			default:
				r = color.v;
				g = p;
				b = q;
				break;
		}
	}

	converted.r = r * maxValue;
	converted.g = g * maxValue;
	converted.b = b * maxValue;
	return converted;
}

HSV RGBtoHSV(RGB color, int maxValue) {
	HSV converted;
	float min, max, delta;
	float r, g, b;

	r = (float) color.r / (float) maxValue;
	g = (float) color.g / (float) maxValue;
	b = (float) color.b / (float) maxValue;

	min = MIN(r, g, b);
	max = MAX(r, g, b);
	converted.v = max;

	delta = max - min;
	if (max > 0.0) {
		converted.s = delta / max;
	} else {
		// undefined if black
		converted.h = -1;
		converted.s = 0;
		converted.v = 0;
		return converted;
	}

	if (r == max) {
		converted.h = (g - b) / delta;
	} else if (g == max) {
		converted.h = 2 + (b - r) / delta;
	} else {
		converted.h = 4 + (r - g) / delta;
	}

	converted.h *= 60;
	if (converted.h < 0) {
		converted.h += 360;
	}

	return converted;
}

